/* Helsinki University of Technology
 * T-106.530 Embedded Systems
 *
 * Car-project
 * HW abstraction layer
 *
 * Target: Atmel AVR ATmega32
 *
 * Author: Petri Hintukainen, 44020U
 * Date:   2012/03/16
 */

#include "hw.h"
#include "hal.h"

#define DIR_ANGLE_MIN      (-85)
#define DIR_ANGLE_MAX      ( 85)
#define MOTOR_SPEED_LIMIT  (HW_MOTOR_SPEED_MAX / 3)

/*
 * Wait for button press
 */

void car_wait_button_pressed(void)
{
  while (!hw_read_button());
  while (hw_read_button());
}

static int led_state = 0;

void car_set_led(int on)
{
  led_state = on;
  hw_set_led(led_state);
}

void car_toggle_led(void)
{
  led_state = !led_state;
  hw_set_led(led_state);
}


/*
 * Sleep
 */

void sleep_us(uint64_t us)
{
  uint64_t time_finished = hw_time_us() + us;
  while (hw_time_us() < time_finished);
}

/*
 * Set car direction
 * angle: -85 ... 85 deg
 */


void car_set_direction(int8_t angle)
{
  /* -85 .. +85 degrees */

  if (angle < DIR_ANGLE_MIN)
    angle = DIR_ANGLE_MIN;
  if (angle > DIR_ANGLE_MAX)
    angle = DIR_ANGLE_MAX;

  /* convert degrees to 0...255 */
  hw_servo_set( angle + (-DIR_ANGLE_MIN) * 0xff / (DIR_ANGLE_MAX - DIR_ANGLE_MIN));
}

void car_stop(void)
{
  hw_motor_speed(0);
  // wait until stopped
}

void car_set_speed(int8_t speed)
{
  // convert to -255...255

#warning todo use interrupt handler to keep speed in m/s by driving motor pwm power

  /* limit speed */
  if (speed >= MOTOR_SPEED_LIMIT)
    speed = MOTOR_SPEED_LIMIT;
  if (speed <= -MOTOR_SPEED_LIMIT)
    speed = -MOTOR_SPEED_LIMIT;

#warning todo  /* limit going from fast worward to fast backward etc. */

  hw_motor_speed(speed);
}


/*
 * LCD
 */

void lcd_goto(uint8_t line, uint8_t col)
{
  if (!line)
    hw_lcd_ctrl(0x80 | col); // first byte of first line
  else
    hw_lcd_ctrl(0xc0 | col); // first byte of first line
}

static void _lcd_init(void)
{
  //hw_lcd_ctrl(0x03);
  //sleep_us(5000);
  //hw_lcd_ctrl(0x03);
  //sleep_us(5000);
  //hw_lcd_ctrl(0x03);
  //sleep_us(5000);


  sleep_us(5000);
  //lcd_ctrl(0x28);  // 4-bit bus, 
  hw_lcd_ctrl(0x20);  // 4-bit bus, 

  sleep_us(5000);
  hw_lcd_ctrl(0x08); // display off

  sleep_ms(500);

  //lcd_ctrl(0x0c); // display on
  hw_lcd_ctrl(0x0c); // display on, cursor on, blink

  sleep_us(5000);
  hw_lcd_ctrl(0x01); // clear

  sleep_us(5000);
  hw_lcd_ctrl(0x06); // cursor movement dir, no display shift

  sleep_us(5000);

  hw_lcd_ctrl(0x01); // clear display
  sleep_us(10000);
}

void lcd_string(const char *s)
{
  lcd_goto(0, 0);
  sleep_us(1000);

  while (*s) {
    hw_lcd_byte(*s);
    sleep_us(10);
    s++;
  }

  sleep_us(1000);
//  lcd_ctrl(0x02); // cursor home
  sleep_us(3000);
}

void car_init(void)
{
  hw_init();

  _lcd_init();
}

void car_reset(void)
{
  hw_init();
}

